#include "rclcpp/rclcpp.hpp" 
#include "nav_msgs/msg/path.hpp" 
#include "geometry_msgs/msg/pose_stamped.hpp" 
#include <cmath>
 
using namespace std::chrono_literals;
 
class PathPublisher : public rclcpp::Node {
public:
    PathPublisher() : Node("path_publisher") {
        // 创建路径发布者 
        publisher_ = this->create_publisher<nav_msgs::msg::Path>("trajectory", 10);
        
        // 定时器生成轨迹数据 
        timer_ = this->create_wall_timer(
            500ms, std::bind(&PathPublisher::timer_callback, this));
         
        RCLCPP_INFO(this->get_logger(), "Trajectory publisher started");
    }
 
private:
    void timer_callback() {
        auto path_msg = nav_msgs::msg::Path();
        path_msg.header.frame_id  = "map";  // 必须与TF树匹配 
        path_msg.header.stamp  = this->now();
 
        // 生成圆周轨迹示例 
        constexpr int POINTS = 50;
        constexpr double RADIUS = 1.0;
        for(int i=0; i<POINTS; ++i){
            geometry_msgs::msg::PoseStamped pose;
            pose.header  = path_msg.header; 
            
            double theta = 2*M_PI*i/POINTS;
            pose.pose.position.x  = RADIUS * cos(theta);
            pose.pose.position.y  = RADIUS * sin(theta);
            pose.pose.orientation.w  = 0.5;  // 无旋转 
            
            path_msg.poses.push_back(pose); 
        }
 
        publisher_->publish(path_msg);
    }
 
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};
 
int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PathPublisher>());
    rclcpp::shutdown();
    return 0;
}
